# -*- coding: utf-8 -*-
"""
Created on Sun Sep  8 01:25:11 2024

@author: LENOVO
"""

# -*- coding: utf-8 -*-
"""
Created on Sat Sep  7 12:28:57 2024

@author: LENOVO
"""
import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd
import math

H=1.7
a=16*H  #m
b=H/(2*np.pi) #m
vh=1 #m/s
L0=(341-27.5*2)/100
Lb=(220-27.5*2)/100

r1=lambda theta:((a-b*(theta)))
# r2=lambda theta:((a-b*(theta+np.pi)))

f1=lambda theta:((a-b*(theta)))-4.5
theta1=root(f1,0).x[0] #进入调头空间角度
#进入掉头空间时间
t1=(b*theta1-a)*np.sqrt((a-b*theta1)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*theta1)**2+b**2)+a-b*theta1)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))
#进入掉头空间时 龙头位置

#某点斜率函数
def Slope(ta):
    k=(-a*np.cos(ta)+b*np.sin(ta)+b*ta*np.cos(ta))/(-a*np.sin(ta)-b*np.cos(ta)+b*ta*np.sin(ta))
    return k
# theta=np.linspace(0,32*np.pi,30*180)


# ax=plt.subplot(111, polar=True)
# ax.set_theta_direction(-1)
# plt.plot(theta,r(theta),lw=1,c='r', label='原始数据点')
# plt.legend()
# plt.title('非线性最小二乘拟合')
# plt.grid()  
# plt.show()




thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值

def theta(t):
    f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))-t
    theta=root(f,thetas(t))
    theta=theta.x[0]
    return theta


def position(i):
    
    r=r1
    Data=[]
    THETA=[]
    X=[]
    Y=[]
    # y=np.ones((301,1))
    
    THETA.append(theta(i))
    X.append(r(THETA[0])*np.cos((THETA[0])))
    Y.append(-r(THETA[0])*np.sin((THETA[0])))

    Data.append(theta(i))
    f=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f,THETA[0]-0.5)
    thetai=thetai.x[0]
    
    # print("theta1:",thetai)
    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    Data.append(thetai)
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))

    A=np.column_stack((X, Y, THETA))

    return A

def Loonghead(t,tx):
    theta0=theta(t)
    P0=position(t)
    x0=P0[0,0]
    y0=P0[0,1]
    k0=Slope(theta0)
    k1=-1/(y0/x0)
    tan=(k1-k0)/(1+k0*k1)
    fai=np.arctan(abs(tan))
    R=r1(theta0)/(3*np.cos(fai))
    fo1=lambda x:[(y0-x[1])/(x0-x[0])+k0,(x0-x[0])**2+(y0-x[1])**2-(2*R)**2]
    o1=fsolve(fo1,[x0-0.1,y0-0.1])
    # fo2=lambda x:[(-y0-x[1])/(-x0-x[0])+k0,(-x0-x[0])**2+(-y0-x[1])**2-(R)**2]
    # o2=fsolve(fo2,[-x0-0.1,-y0-0.1])
    # o=np.vstack((o1,o2)) #两个圆心原点坐标 第一个为2R 第二个为R
    
    
    # #大圆圆心坐标系下ABC三点坐标
    # xA=-2*R*np.cos(fai)
    # yA=2*R*np.sin(fai)
    # xB=2*R*cos(fai)
    # yB=2*R*np.sin(fai)
    # xC=4*R*np.cos(fai)
    # yC=2*R*np.sin(fai)
    
    #坐标变换矩阵 
    B=np.array([[-x0/r1(theta0),y0/r1(theta0)],[-y0/r1(theta0),-x0/r1(theta0)]])
    #螺旋线到圆弧坐标
    def LTY(Y):
        L=B@Y+o1.T
        return L
    # return(LTY(np.array([xA,yA]))) #测试点
    
    #时间节点
    t0=t
    t1=2*R*(np.pi-2*fai)/vh+t0
    t2=R*(np.pi-2*fai)/vh+t1
    # deltat=t2-t0
    #龙头位置：龙头在螺旋线坐标系下坐标 
    #未进入圆弧前 
    if (tx>=0 and tx<t0):
        P=position(t)
        x=P[0,0]
        y=P[0,1]
        GPS=np.array([x,y])
    #第一段圆弧
    elif (tx>=t0 and tx<t1):
        x=2*R*np.cos(np.pi-fai-vh*(t-t0)/(2*R))
        y=2*R*np.sin(np.pi-fai-vh*(t-t0)/(2*R))
        ZB=np.array([x,y])
        GPS=LTY(ZB.T)
    #第二段圆弧    
    elif (tx>=t1 and tx<t2):
        x=R*np.cos(np.pi-fai-vh*(t-t1)/R)+3*R*np.cos(fai)
        y=-R*np.sin(np.pi-fai-vh*(t-t1)/R)+3*R*np.sin(fai)
        ZB=np.array([x,y])
        GPS=LTY(ZB.T)
    #出圆弧后 由中心对称可知
    else:
        P=position(t0+t2-t)
        x=-P[0,0]
        y=-P[0,1]
        GPS=np.array([x,y])
    return GPS
#第ts开始进入调头曲线
# def Turn(t):
#     theta0=theta(t)
#     P0=position(t)
#     x0=P0[0,0]
#     y0=P0[0,1]
#     k0=Slope(theta0)
#     k1=-1/(y0/x0)
#     tan=(k1-k0)/(1+k0*k1)
#     fai=np.arctan(abs(tan))
#     R=r1(theta0)/(3*np.cos(fai))
#     fo1=lambda x:[(y0-x[1])/(x0-x[0])+k0,(x0-x[0])**2+(y0-x[1])**2-(2*R)**2]
#     o1=fsolve(fo1,[x0-0.1,y0-0.1])
#     fo2=lambda x:[(-y0-x[1])/(-x0-x[0])+k0,(-x0-x[0])**2+(-y0-x[1])**2-(R)**2]
#     o2=fsolve(fo2,[-x0-0.1,-y0-0.1])
#     o=np.vstack((o1,o2)) #两个圆心原点坐标 第一个为2R 第二个为R
    
    
#     #大圆圆心坐标系下ABC三点坐标
#     xA=-2*R*np.cos(fai)
#     yA=2*R*np.sin(fai)
#     xB=2*R*cos(fai)
#     yB=2*R*np.sin(fai)
#     xC=4*R*np.cos(fai)
#     yC=2*R*np.sin(fai)
    
#     #坐标变换矩阵 
#     B=np.array([[-x0/r1(theta0),y0/r1(theta0)],[-y0/r1(theta0),-x0/r1(theta0)]])
#     #螺旋线到圆弧坐标
#     def LTY(Y):
#         L=B@Y+o1.T
#         return L
#     # return(LTY(np.array([xA,yA]))) #测试点
    
#     #时间节点
#     t0=t
#     t1=2*R*(np.pi-2*fai)/vh+t0
#     t2=R*(np.pi-2*fai)/vh+t1
#     deltat=t2-t0
#     #龙头位置：龙头在螺旋线坐标系下坐标 
#     def Loonghead (t,t0):
#         #未进入圆弧前 
#         if (t>=0 and t<t0):
#             P=position(t)
#             x=P[0,0]
#             y=P[0,1]
#             GPS=np.array([x,y])
#         #第一段圆弧
#         elif (t>=t0 and t<t1):
#             x=2*R*np.cos(np.pi-fai-vh*(t-t0)/(2*R))
#             y=2*R*np.sin(np.pi-fai-vh*(t-t0)/(2*R))
#             ZB=np.array([x,y])
#             GPS=LTY(ZB.T)
#         #第二段圆弧    
#         elif (t>=t1 and t<t2):
#             x=R*np.cos(np.pi-fai-vh*(t-t1)/R)+3*R*np.cos(fai)
#             y=-R*np.sin(np.pi-fai-vh*(t-t1)/R)+3*R*np.sin(fai)
#             ZB=np.array([x,y])
#             GPS=LTY(ZB.T)
#         #出圆弧后 由中心对称可知
#         else:
#             P=position(t0+t2-t)
#             x=-P[0,0]
#             y=-P[0,1]
#             GPS=np.array([x,y])
#         return GPS
#     LTQ=[]
#     for i in range(0,int(t)+1,1):
#         LTQ.append(Loonghead(i,1330))
#     return LTQ #测试点

# LTQ=Turn(1330)
# LTQ=pd.DataFrame(LTQ)
# LTQ.to_excel('1300.xlsx',index=False,header=False)


# LTQ=[]
# for i in range(0,1301,1):
#     LTQ.append(Loonghead(1330,i))
# LTQ=pd.DataFrame(LTQ)
# LTQ.to_excel('1300.xlsx',index=False,header=False)    
            
            
            
    
        
        
        
    # return o
    
    
    
    